import time


class PID:
    """
    PID Controller
    """

    def __init__(self, max=255, min=60, P=0.4, I=0.02, D=0.01, current_time=None, sample_interval=50):

        self.max = max
        self.min = min

        self.Kp = P
        self.Ki = I
        self.Kd = D

        self.sample_interval = sample_interval
        self.current_time = current_time if current_time is not None else time.time()
        self.last_time = self.current_time

        self.PTerm_list = []
        self.ITerm_list = []
        self.DTerm_list = []

        """Clears PID computations and coefficients"""
        self.targetPoint = 0

        self.PTerm = 0
        self.ITerm = 0
        self.DTerm = 0
        self.last_error = 0

        # Windup Guard
        # 积分误差上限
        self.windup_guard = 20

        self.output = 0

    def update(self, feedback_value, current_time=None):
        """Calculates PID value for given reference feedback

        .. math::
            u(t) = K_p e(t) + K_i \int_{0}^{t} e(t)dt + K_d {de}/{dt}

        .. figure:: images/pid_1.png
           :align:   center

           Test PID with Kp=1.2, Ki=1, Kd=0.001 (test_pid.py)

        """
        error = self.targetPoint - feedback_value

        self.current_time = current_time if current_time is not None else time.ticks_ms()
        delta_time = time.ticks_diff(self.current_time, self.last_time)
        delta_error = error - self.last_error

        if delta_time >= self.sample_interval:
            self.PTerm = self.Kp * error
            self.ITerm += error * delta_time

            if self.ITerm < -self.windup_guard:
                self.ITerm = -self.windup_guard
            elif self.ITerm > self.windup_guard:
                self.ITerm = self.windup_guard

            self.DTerm = 0
            if delta_time > 0:
                self.DTerm = delta_error / delta_time

            # Remember last time and last error for next calculation
            self.last_time = self.current_time
            self.last_error = error

            self.output = self.PTerm + self.Ki * self.ITerm + self.Kd * self.DTerm
        # self.PTerm_list.append(self.PTerm)
        # self.ITerm_list.append(self.Ki * self.ITerm)
        # self.DTerm_list.append(self.Kd * self.DTerm)
        self.output = self.targetPoint - self.output
        self.output = min(self.max, self.output)
        self.output = max(self.min, self.output)
        return int(self.output)

    def get_values(self):
        return self.PTerm_list, self.ITerm_list, self.DTerm_list

    def set_target(self, target_value):
        self.targetPoint = target_value

    def set_kp(self, proportional_gain):
        """Determines how aggressively the PID reacts to the current error with setting Proportional Gain"""
        self.Kp = proportional_gain

    def set_ki(self, integral_gain):
        """Determines how aggressively the PID reacts to the current error with setting Integral Gain"""
        self.Ki = integral_gain

    def set_kd(self, derivative_gain):
        """Determines how aggressively the PID reacts to the current error with setting Derivative Gain"""
        self.Kd = derivative_gain

    def set_windup(self, windup):
        """Integral windup, also known as integrator windup or reset windup,
        refers to the situation in a PID feedback controller where
        a large change in setpoint occurs (say a positive change)
        and the integral terms accumulates a significant error
        during the rise (windup), thus overshooting and continuing
        to increase as this accumulated error is unwound
        (offset by errors in the other direction).
        The specific problem is the excess overshooting.
        """
        self.windup_guard = windup

    def set_sample_interval(self, sample_interval):
        """PID that should be updated at a regular interval.
        Based on a pre-determined sampe time, the PID decides if it should compute or return immediately.
        """
        self.sample_interval = sample_interval
